/**
  ******************************************************************************
  * @file           : studio_full_path_coverage.h
  * @author         : wangyingjie
  * @brief          : 路径全覆盖算法 采用水波曲线的形式进行扫描覆盖
  * @attention      : None
  * @date           : 2025/6/8
  ******************************************************************************
  */

#ifndef STUDIO_WAVE_COVER_CCPP_H
#define STUDIO_WAVE_COVER_CCPP_H

#include "studio_macros.h"

using Position = std::pair<int, int>;
const int AO_INF = 10000000;

/**
 * 定义地图上的一个点的数据结构
 * 该结构体用于存储与地图上的一个特定点相关的各种信息，包括其占用状态、成本、访问状态等
 */
struct mapPoint
{
    double occupancy = AO_INF;                               // 当前点的占用概率，初始值设置为无穷大，表示未初始化
    double cost = 0.0;                                       // 从起点到当前点的路径成本，初始值为0
    double exitCost = 0.0;                                   // 从当前点到出口的成本，用于计算最短路径，初始值为0
    bool isVisited = false;                                  // 标记当前点是否已被访问过，初始值为false
    bool costComputed = false;                               // 标记当前点的成本是否已计算，初始值为false
    bool exitChecked = false;                                // 标记当前点是否已检查过出口，初始值为false
    bool isTracked = false;                                  // 标记当前点是否被追踪，用于某些特定的算法或处理，初始值为false
    int visitedNum = 0;                                      // 当前点被访问的次数，用于统计和优化路径规划，初始值为0
    Position next_point_position = Position(AO_INF, AO_INF); // 下一个点的位置，用于指示路径方向，初始值设置为无穷远
    Position prev_point_position = Position(AO_INF, AO_INF); // 前一个点的位置，用于回溯路径，初始值设置为无穷远
};

/// 波前全覆盖规划器
class studio_wave_cover_ccpp
{
public:
    studio_wave_cover_ccpp(int width, int height)
        : MAP_WIDTH(width),
          MAP_HEIGHT(height)
    {
        prob_map.clear();
        candidate_positions.clear();
        optimal_path.clear();
        isExitFound = false;
        isPlanningFinished = false;

        // 初始化地图所有点为障碍
        for (int x = 0; x < MAP_WIDTH; x++)
        {
            for (int y = 0; y < MAP_HEIGHT; y++)
            {
                Position pos(x, y);
                prob_map[pos] = mapPoint();
                prob_map[pos].occupancy = AO_INF;
            }
        }
    }

    // 设置电子围栏区域
    void setFencedArea(const std::map<int, std::vector<int>>& all_block);

    // 设置起点
    void setStartPosition(Position start);

    // 获取规划路径
    std::vector<Position> getOptimalPath();

    /**
     * 获取指定位置的邻居位置
     *
     * 此函数用于计算并返回一个指定位置周围的所有邻居位置
     * 它主要用于在二维网格或地图上找到某个点的所有相邻点
     *
     * @param root_outosition 基准位置，用于计算其周围邻居的位置
     * @return 包含所有邻居位置的向量
     */
    std::vector<Position> getNeighbors(Position pos);

    /**
     * 更新邻近位置的成本信息
     *
     * @param root_outosition 当前位置，以此位置为中心更新其周围邻居的位置信息
     *
     * 此函数的主要作用是更新当前位置周围8个邻居位置的成本信息如果邻居位置的有效且未被计算过成本，
     * 则更新其成本信息并将该位置添加到候选位置列表中以供后续处理
     */
    void updateNeighbors(Position root_outosition);

    /**
     * 使用波前算法传播成本
     *
     * 该函数从一个起始位置开始，逐步更新其周围位置的成本信息，直到所有可到达的位置都被更新
     * 它主要用于路径规划和成本计算
     *
     * @param start_position 起始位置，表示为二维坐标
     */
    void waveFrontSpread(Position start_position);

    /**
     * 重置地图上各位置的出口成本及相关状态
     *
     * 此函数遍历地图上的每一个位置，并重置与出口成本计算相关的状态信息
     * 包括是否检查过出口、出口成本、是否被追踪等属性这些状态信息
     * 对于路径规划和成本计算至关重要，通过重置这些值，可以准备
     * 对地图状态进行新一轮的评估或重置当前的评估结果
     */
    void resetExitCost();

    /**
     * 寻找出口位置
     *
     * @param stuck_position 被困的位置
     * @return Position 找到的出口位置，如果找不到则返回无穷远的坐标
     *
     * 此函数尝试从被困位置找到一个出口，通过检查周围8个邻居位置来确定
     * 是否找到了出口，或者是否有潜在的路径可以通往出口
     */
    Position findExit(Position stuck_position);

    /**
     * 当机器人被困在角落时，尝试找到逃脱路径的函数
     * 使用 Dijkstra 算法从角落逃离。
     * @param stuck_position 机器人被困的位置
     */
    void cornerEscape(Position stuck_position);

    /**
     * 寻找下一步的最佳位置
     *
    * 检查当前位置的 8 个邻居
        跳过：地图外位置 / 障碍物 / 已访问点
        选择 成本最小 (min_cost) 的未访问邻居，如果无法找到更优位置，则调用角落逃脱函数
     *
     * @param position 当前位置，表示为(std::pair<int, int>)，其中first和second分别表示行和列
     */
    void findNextStep(Position position);

    /**
     * 执行完整覆盖路径规划算法
     *
     * 该函数从给定的起始位置开始，利用波前展开法进行路径规划，直到路径规划完成
     * 它通过在概率地图上标记访问过的区域并计算访问次数来帮助机器人覆盖所有区域
     */
    void planPath();

private:
    // 定义地图的高度
    int MAP_HEIGHT;
    // 定义地图的宽度
    int MAP_WIDTH;
    Position start_position = {AO_INF, AO_INF};        // 起点位置
    std::deque<Position> shortcut_candidate_positions; // 存储快捷方式候选位置的双端队列
    std::deque<Position> sub_path;                     // 存储子路径的双端队列
    bool isExitFound = false;                          // 标记是否找到出口
    bool isPlanningFinished = false;                   // 标记路径规划是否完成

    std::map<Position, mapPoint> prob_map;    // 障碍物地图
    std::deque<Position> candidate_positions; // 候选位置队列
    std::vector<Position> optimal_path;       // 最优路径
};

// std::map<int, std::vector<int>> all_block = xlr_gcs.get_blocks();
// studio_wave_cover_ccpp planner(xlr_gcs.width(), xlr_gcs.height());
// // 设置电子围栏区域
// planner.setFencedArea(all_block);
// // 正确获取起始点坐标
// int start_y = all_block.begin()->first; // 行号 (y坐标)
// int start_x = all_block.begin()->second.at(0); // 列号 (x坐标)
// planner.setStartPosition({start_x, start_y}); // (x, y)
// // 执行路径规划
// planner.planPath();
// std::vector<Position> grad_path = planner.getOptimalPath();

#endif //STUDIO_WAVE_COVER_CCPP_H